clear all;
close all; clc;
%*******************************************************************
%% SELECT DATA PARAMETERS
%*******************************************************************
Ang_mat(:,1)=[-50.1, 50.3,  10.5];      % phi
Ang_mat(:,2)=[ 5.2, -40.1, 11.3];       % psi
Pol_mat(:,1)=[ 20.7, 40.7, -20.7];      % alpha
Pol_mat(:,2)=[ -10.3, 30.4, -10.1];     % beta
Ang_sort = sort(Ang_mat(:,1));
K=length(Ang_mat(:, 1));    % number of signals
fc = 1e9;                   % carrier frequency in Hz
c = 3e8;                    % speed of light (m/s)
Lambda = c/fc;              % wavelength (m)
dr=Lambda/2;                % inter-element spacing at ULA receiver (m)
dplamda = dr/Lambda;
u_vec = poyvec(Ang_mat);	% generate poyvec
Mr = 5;                       % number of receive sensors
Q = 6;                        % number of polarizing antennas
% Monte Carlo times
Mont = 50;                     
% SNR value:5,10,15,20,25,30
SNR0 = [-15, -12, -9, -6, -3, 0, 3, 6, 9, 12, 15];
%   Snapshot numbers
L0 = [10,20,50,100,200,500,1000,2000,5000,1e4];

leng_var = length(SNR0); Xlabl=SNR0; L=20;  % Test of SNR, switch
% leng_var = length(L0); Xlabl=L0; SNR = 3; % Test of Snapshot, switch
[time_PARAFAC_MC, time_esprit_MC, time_music_MC...
    err_Ang_parafac_MC, err_Ang_esprit_MC, err_Ang_music_MC]...
    = deal(zeros(leng_var, Mont));
[Ang_parafac_MC, Ang_esprit_MC, Ang_music_MC] = deal(zeros(3, Mont));

for mont = 1:Mont
for ith = 1:leng_var
    SNR = SNR0(ith)  % Test of SNR, switch
%     L=L0(ith)   % Test of Snapshot, switch
%*******************************************************************
%% GENERATE DATA
%*******************************************************************
S = randn(K, L) + 1i*randn(K, L); % source signal
S = S.';
% EM polarizaton matrix B(Q*K)
B = EM_mani(Ang_mat, Pol_mat, Q, K);
A = exp( 1i*2*pi*(0:Mr-1)'*dplamda*sin( Ang_mat(:, 1).'*pi/180 ) );
%    Generate noise-free observed tensor of size Mr*L*Q
X_rec=zeros(Mr,L,Q);
for q=1:Q
    X_rec(:, :, q) = A * diag(B(q, :)) * S.';
end
% X_rec = kr_prod(B, A, S);   % K-R produc(as same as above)
%   Add noise
Noise_tens = randn(Mr,L,Q) + 1i*randn(Mr,L,Q);
sigma = (10^(-SNR/10)) * ( norm(X_rec(:), 'fro')/sqrt(2*Mr*L*Q) );
%    Generate noisy observed tensor of size Mr*L*Q
Y_rec = X_rec + sigma*Noise_tens;
%*******************************************************************
%START TARGET LOCALIZATION
%*******************************************************************
%% Implement algorithms (record err_Ang and time in a leng_var*Mont Mat respectively)
end  % The end of iteration
end  % The end of Mont
% calculate the mean result in a leng_var*1 vector
err_Ang_parafac = mean(err_Ang_parafac_MC, 2);
err_Ang_esprit = mean(err_Ang_esprit_MC, 2);
err_Ang_music = mean(err_Ang_music_MC, 2);
time_PARAFAC = mean(time_PARAFAC_MC, 2);
time_esprit = mean(time_esprit_MC, 2);
time_music = mean(time_music_MC, 2);
%********************************************************************
%% PLOTS of Angle estimates
%********************************************************************
% plot the computing time
figure
semilogy(time_PARAFAC, '-o', 'LineWidth', 2); hold on;
semilogy(time_esprit, 'r-o', 'LineWidth', 2); 
semilogy(time_music, 'g-o', 'LineWidth', 2); 
grid on; axis normal
set(gca,'xtick', [1:1:leng_var])
set(gca,'xticklabel', Xlabl)
xlabel({['SNR(db)']; ['(with M, K equal to ', num2str(Mr),', ',num2str(K),...
    ', by ', num2str(Mont), ' Monte Carlo simulations)']})    % switch
% xlabel({['Snapshots']; ['(with M, K equal to ', num2str(Mr),', ',num2str(K),...
%     ', by ', num2str(Mont), ' Monte Carlo simulations)']})    % switch
ylabel('Time(s)')
title('Computing Time')
legend('PARAFAC','ESPRIT','MUSIC', 'Location','Best')
hold off

% plot the MSE
figure
% %   Spline curve smoothing
% values = spcrv([[SNR0(1) SNR0 SNR0(end)];[err_Ang(1) err_Ang err_Ang(end)]],3);
% plot(values(1,:), values(2,:), 'LineWidth', 1);
semilogy(err_Ang_parafac, '-o', 'LineWidth', 2); hold on;
semilogy(err_Ang_esprit, 'r-o', 'LineWidth', 2);
semilogy(err_Ang_music, 'g-o', 'LineWidth', 2);
% plot( err_Ang_parafac, '-o', 'LineWidth', 2); hold on;
% plot( err_Ang_esprit, 'r-o', 'LineWidth', 2);
grid on; axis normal
set(gca,'xtick', [1:1:leng_var])
set(gca,'xticklabel', Xlabl)
xlabel({['SNR(db)']; ['(with M, K equal to ', num2str(Mr),', ',num2str(K),...
    ', by ', num2str(Mont), ' Monte Carlo simulations)']})  % switch
% xlabel({['Snapshots']; ['(with M, K equal to ', num2str(Mr),', ',num2str(K),...
%     ', by ', num2str(Mont), ' Monte Carlo simulations)']})    % switch
ylabel('MSE')
title('The MSE of 3-Way Methods')
legend('PARAFAC','ESPRIT','MUSIC','Location','Best')
hold off